
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <iostream>
#include "solve.h"
#include <cmath>

using namespace std;

/// Solves a linear system of the form a x = b using LU decomposition
/// @arg a pointer to an array of doubles containing a square matrix stored row by row
/// @arg b pointer to an array of doubles containing the independent terms for each row
/// @arg x pointer to an array of doubles which will contain the result 
/// @arg n number of equations in the system, i.e., rows and columns of a, b, and x
/// @return True if the system is not singular
bool solve (double* a, double * b, double * x, size_t n)
{
    gsl_matrix_view am = gsl_matrix_view_array (a, n, n);
    gsl_vector_view bv = gsl_vector_view_array (b, n);
    gsl_vector_view xv = gsl_vector_view_array (x, n);

    /* Operate on a copy of a since the LU decomposition destroys the original matrix */
    gsl_matrix* acopy = gsl_matrix_alloc(n,n);
    gsl_matrix_memcpy (acopy, &am.matrix);

    int s;

    gsl_permutation * p = gsl_permutation_alloc (n);
    gsl_linalg_LU_decomp (acopy, p, &s);
    gsl_linalg_LU_solve (acopy, p, &bv.vector, &xv.vector);
    
    /* Free memory */
    gsl_permutation_free (p);
    gsl_matrix_free (acopy);
    
    for (unsigned i = 0; i < n; i++) {
      if (isnan (x[i])) return false;
    }
    return true;
}

/// Inverts a square n x n matrix
/// @arg a pointer to an array of doubles containing a square matrix stored row by row
/// @arg inv pointer to an array of doubles which will contain the inverse of a
/// @arg n number of rows and columns in both a and inv
/// @return True if inversion was possible (non-singular matrix)
bool invert(double* a, double* inv, size_t n)
{
    gsl_matrix_view am = gsl_matrix_view_array (a, n, n);

    /* Operate on a copy of a since the LU decomposition destroys the original matrix */
    gsl_matrix* acopy = gsl_matrix_alloc(n,n);
    gsl_matrix_memcpy (acopy, &am.matrix);
    
    int s;

    gsl_permutation * p = gsl_permutation_alloc (n);
    
    /* calculate the inverse */
    gsl_matrix_view inverse = gsl_matrix_view_array (inv, n, n);
    gsl_linalg_LU_decomp(acopy, p, &s);
    return gsl_linalg_LU_invert(acopy, p, &inverse.matrix) == 0;
    
}

/// Multiplies two square matrices
/// @arg a matrix of size n x n stored row by row in an array.
/// @arg b matrix of size n x n stored row by row in an array.
/// @arg c matrix of size n x n stored row by row in an array containing the product of a and b.
/// @arg n number of rows and columns.
void matmatmult (double* a, double *b, double *c, size_t n)
{
    gsl_matrix_view ma = gsl_matrix_view_array (a, n, n);
    gsl_matrix_view mb = gsl_matrix_view_array (b, n, n);
    gsl_matrix_view mc = gsl_matrix_view_array (c, n, n);

    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, &ma.matrix, &mb.matrix, 0, &mc.matrix);
}

/// Multiplies a square matrix by a vector returning a vector
/// @arg a matrix of size n x n stored row by row in an array.
/// @arg b vector of size n.
/// @arg c vector of size ncontaining the product of a and b.
/// @arg n number of rows and columns.
void matvecmult (double* a, double *b, double *c, size_t n)
{
   for (unsigned i = 0; i < n; i++) {
      c[i] = 0;
      for (unsigned j = 0; j < n; j++) {
         c[i] += a[i*n+j]*b[j];
      }
   }
}
